#include "sim_move.h"
#include "air_motion_lib.h"
#include <stdio.h>


static int32_t x_offset = 0, y_offset = 0;

void motion_detect_init(void)
{
    t_struct_AIR_MOTION_Init lInitParameters;
    
    lInitParameters.DeltaGain.X             = 15;
    lInitParameters.DeltaGain.Y             = 15;
    lInitParameters.GyroOffsets.X           = 0;
    lInitParameters.GyroOffsets.Y           = 0;
    lInitParameters.GyroOffsets.Z           = 0;
    lInitParameters.GyroStaticMaxNoise      = 16;
    lInitParameters.StaticSamples           = 400;
    lInitParameters.SwipeMinDist            = 250;
    lInitParameters.SwipeMaxNoise           = 200;
    lInitParameters.StartupSamples          = 16;
    lInitParameters.ClickStillSamples       = 30;
    lInitParameters.ClickStillTolerance     = AirMotionNormal;
    lInitParameters.IsRollCompEnabled       = true;
    lInitParameters.Acc1gLsb                = 16384;//256;
    lInitParameters.GyroSensitivity         = 262;
    
    AIR_MOTION_Init(&lInitParameters);
	
	x_offset = 0;
	y_offset = 0;
}


void motion_detect(short *accel, short *gyro)
{
	static uint8_t old_swipe = 0;
    t_struct_AIR_MOTION_ProcessDeltaSamples  lSensorSamples;
    t_struct_AIR_MOTION_ProcessDeltaStatus   lProcessDeltaStatus;

	lSensorSamples.AccSamples.X = accel[0];
	lSensorSamples.AccSamples.Y = accel[1];
	lSensorSamples.AccSamples.Z = accel[2];
	
	lSensorSamples.GyroSamples.X = gyro[0];
	lSensorSamples.GyroSamples.Y = gyro[1];
	lSensorSamples.GyroSamples.Z = gyro[2];
	
	lSensorSamples.ClickSample = 0;
	
	lProcessDeltaStatus = AIR_MOTION_ProcessDelta(lSensorSamples);
        
//        if((lProcessDeltaStatus.SwipesDetected.swipes & 0x3f) != 0)
//        {
//            printf("Action:0x%02X\r\n", (uint16_t)(lProcessDeltaStatus.SwipesDetected.swipes));
//            AIR_MOTION_ResetSwipeRecognition();
//            return;
//        }


	if(lProcessDeltaStatus.Status.IsDeltaComputed)
	{
		x_offset += lProcessDeltaStatus.Delta.X;
		y_offset += lProcessDeltaStatus.Delta.Y;
		
		printf("%d,%d\r\n", x_offset, y_offset);
		//printf("Delta:%d,%d\r\n", (lProcessDeltaStatus.Delta.X),(lProcessDeltaStatus.Delta.Y));
	}
	
	if(lProcessDeltaStatus.Status.IsStatic)
	{
		//x_offset = 0;
		//y_offset = 0;
		// printf("IsStatic\r\n");
	}
	
	if(lProcessDeltaStatus.Status.NewGyroOffset)
	{
		//printf("New offset:%d,%d,%d\r\n", lProcessDeltaStatus.GyroOffsets.X, lProcessDeltaStatus.GyroOffsets.Y, lProcessDeltaStatus.GyroOffsets.Z);
	}
	
	if(old_swipe != lProcessDeltaStatus.SwipesDetected.swipes)
	{
		printf("Swipes:%X\r\n", lProcessDeltaStatus.SwipesDetected.swipes);
		old_swipe = lProcessDeltaStatus.SwipesDetected.swipes;
	}
        
	//printf("%X, %X, %X\r\n", accel[0], accel[1], accel[2]);
	//printf("%X, %X, %X\r\n", gyro[0], gyro[1], gyro[2]);
	//printf("Action:0x%02X\r\n", (uint16_t)(lProcessDeltaStatus.SwipesDetected.swipes));
}
